#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// OLED显示屏配置
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
#define SDA_PIN 27
#define SCL_PIN 26
TwoWire I2Cone = TwoWire(0);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &I2Cone, OLED_RESET);

// 定义引脚
#define THROTTLE_PIN 34
#define JOYSTICK_X_PIN 35
#define JOYSTICK_Y_PIN 32
#define POTENTIOMETER_PIN 33
#define THROTTLE_SWITCH_PIN 25
#define JOYSTICK_BUTTON_PIN 4

// 全局变量
int throttleValue = 0;
int joystickX = 0;
int joystickY = 0;
int potentiometerValue = 0;
int throttleSwitchState = 0;
String joystickDirection = "CENTER";
int motor1Speed = 0;
int motor2Speed = 0;

// 定义BTS7960控制引脚
const int RPWM1 = 12;
const int LPWM1 = 13;
const int RPWM2 = 14;
const int LPWM2 = 16;

// 定义PWM参数
const int freq = 5000;
const int pwmChannelR1 = 4;
const int pwmChannelL1 = 5;
const int pwmChannelR2 = 6;
const int pwmChannelL2 = 7;
const int resolution = 8;

// 定义转向灵敏度（15%）
const float TURN_SENSITIVITY = 0.15;

// 定义转向触发阈值（15度对应的ADC值）
const int TURN_THRESHOLD = 1000;  // 对应约15度的ADC值

// 页面0：优化显示 - 适配128x64小屏幕
void displayPage0() {
  // 取消标题显示
  
  // 调整分隔线位置
  display.drawLine(0, 10, SCREEN_WIDTH, 10, SSD1306_WHITE);
  
  // 显示油门状态 - 减小字体
  display.setTextSize(1);
  display.setCursor(10, 20);
  display.print("Throttle: ");
  display.setTextSize(2);
  display.setCursor(75, 17);
  display.print(throttleSwitchState == LOW ? "ON" : "OFF");
  
  // 显示电位计值（带符号） - 减小字体
  display.setTextSize(1);
  display.setCursor(10, 45);
  display.print("Pot: ");
  display.setTextSize(2);
  display.setCursor(40, 42);
  display.print(potentiometerValue);  // 直接显示电位器值
  
  // 显示摇杆方向（使用ASCII字符替代箭头） - 位置调整
  const int quarterWidth = SCREEN_WIDTH / 4;
  const int quarterHeight = SCREEN_HEIGHT / 4;
  display.setTextSize(3);
  // 向右移动16像素，垂直位置保持不变
  display.setCursor(quarterWidth + 8 + 24 + 16, quarterHeight + 4 + 12);
  if (joystickDirection == "UP") {
    display.print("^");  // 向上箭头用^表示
  } else if (joystickDirection == "DOWN") {
    display.print("v");  // 向下箭头用v表示
  } else if (joystickDirection == "LEFT") {
    display.print("<");  // 向左箭头用<表示
  } else if (joystickDirection == "RIGHT") {
    display.print(">");  // 向右箭头用>表示
  } else {
    display.print("o");  // 居中状态用o表示
  }
}    

// 显示SWJTU开机界面
void displaySWJTULogo() {
  display.clearDisplay();
  display.setTextSize(3);
  display.setCursor(10, 15);
  display.print("SWJTU");
  display.display();
}

// 显示欢迎界面
void displayWelcomeScreen() {
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(10, 15);
  display.println("Control");
  display.setTextSize(1);
  display.setCursor(10, 40);
  display.println("System Ready");
  display.display();
}

// 禁用翻页功能
void checkPageChange() {
  // 空函数，不执行任何操作
}

// 读取所有传感器数据
void readSensors() {
  throttleValue = analogRead(THROTTLE_PIN);
  joystickX = analogRead(JOYSTICK_X_PIN);
  joystickY = analogRead(JOYSTICK_Y_PIN);
  potentiometerValue = analogRead(POTENTIOMETER_PIN);
  throttleSwitchState = digitalRead(THROTTLE_SWITCH_PIN);
  
  // 将电位器值转换为-100到100的范围
  potentiometerValue = map(potentiometerValue, 0, 4095, -100, 100);
}

// 处理传感器数据
void processSensorData() {
  // 确定摇杆方向
  joystickDirection = "CENTER";
  if (joystickX < 300) joystickDirection = "LEFT";
  if (joystickX > 3900) joystickDirection = "RIGHT";
  if (joystickY < 300) joystickDirection = "UP";
  if (joystickY > 3900) joystickDirection = "DOWN";
}

// 控制电机
void controlMotors() {
  // 只有当油门开关打开时才控制电机
  if (throttleSwitchState == LOW) {
    // 用电位器值直接控制电机（范围-100到100）
    int baseSpeed = potentiometerValue;
    
    // 将基础速度从-100到100映射到-255到255
    int baseMotorSpeed = map(baseSpeed, -100, 100, -255, 255);
    
    // 处理摇杆转向 - 仅在超过阈值时触发
    float turnFactor = 0.0;
    if (joystickX < (4095/2 - TURN_THRESHOLD)) {
      // 左转区域：将低于阈值的ADC值映射到-1.0到0
      turnFactor = map(joystickX, 0, 4095/2 - TURN_THRESHOLD, -1.0, 0);
    } else if (joystickX > (4095/2 + TURN_THRESHOLD)) {
      // 右转区域：将高于阈值的ADC值映射到0到1.0
      turnFactor = map(joystickX, 4095/2 + TURN_THRESHOLD, 4095, 0, 1.0);
    }
    
    // 计算左右电机的转向调整量
    float leftAdjustment = turnFactor * TURN_SENSITIVITY;
    float rightAdjustment = -turnFactor * TURN_SENSITIVITY;
    
    // 计算左右电机的最终速度
    int leftMotorSpeed = constrain(baseMotorSpeed * (1 + leftAdjustment), -255, 255);
    int rightMotorSpeed = constrain(baseMotorSpeed * (1 + rightAdjustment), -255, 255);
    
    // 控制两个电机（假设电机1为左电机，电机2为右电机）
    controlMotor(1, leftMotorSpeed);  // 左电机
    controlMotor(2, rightMotorSpeed); // 右电机
    
    // 打印电机控制信息
    Serial.print("Base Speed: ");
    Serial.print(baseSpeed);
    Serial.print("  |  Turn Factor: ");
    Serial.print(turnFactor);
    Serial.print("  |  Left Speed: ");
    Serial.print(leftMotorSpeed);
    Serial.print("  |  Right Speed: ");
    Serial.println(rightMotorSpeed);
  } else {
    // 油门关闭时停止电机
    controlMotor(1, 0);
    controlMotor(2, 0);
  }
}

// 禁用串口指令处理（不再需要）
void processSerialCommands() {
  // 空函数，不执行任何操作
}

// 控制指定电机（motorId=1或2）的函数
void controlMotor(int motorId, int speed) {
  // 限制速度范围在-255到255之间
  speed = constrain(speed, -255, 255);
  
  // 根据电机ID选择对应的PWM通道
  int pwmChannelR, pwmChannelL;
  
  if (motorId == 1) {
    pwmChannelR = pwmChannelR1;
    pwmChannelL = pwmChannelL1;
  } else {
    pwmChannelR = pwmChannelR2;
    pwmChannelL = pwmChannelL2;
  }
  
  // 根据速度控制电机
  if (speed > 0) {
    // 正转
    ledcWrite(pwmChannelR, speed);
    ledcWrite(pwmChannelL, 0);
  } else if (speed < 0) {
    // 反转
    ledcWrite(pwmChannelR, 0);
    ledcWrite(pwmChannelL, abs(speed));
  } else {
    // 停止
    ledcWrite(pwmChannelR, 0);
    ledcWrite(pwmChannelL, 0);
  }
}

void setup() {
  Serial.begin(115200);
  
  // 初始化自定义I2C引脚
  I2Cone.begin(SDA_PIN, SCL_PIN, 400000);
  
  // 初始化OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 display allocation failed"));
    for(;;);
  }
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  
  // 显示开机界面
  displaySWJTULogo();
  delay(2000); // 显示2秒
  
  // 设置输入引脚
  pinMode(THROTTLE_SWITCH_PIN, INPUT_PULLUP);
  pinMode(JOYSTICK_BUTTON_PIN, INPUT_PULLUP);
  
  displayWelcomeScreen();
  delay(1500);

  // 电机控制初始化
  Serial.println("Motor Control System Ready!");
  
  // 设置电机引脚模式
  pinMode(RPWM1, OUTPUT);
  pinMode(LPWM1, OUTPUT);
  pinMode(RPWM2, OUTPUT);
  pinMode(LPWM2, OUTPUT);
  
  // 配置PWM通道
  ledcSetup(pwmChannelR1, freq, resolution);
  ledcSetup(pwmChannelL1, freq, resolution);
  ledcSetup(pwmChannelR2, freq, resolution);
  ledcSetup(pwmChannelL2, freq, resolution);
  
  // 将PWM通道绑定到对应引脚
  ledcAttachPin(RPWM1, pwmChannelR1);
  ledcAttachPin(LPWM1, pwmChannelL1);
  ledcAttachPin(RPWM2, pwmChannelR2);
  ledcAttachPin(LPWM2, pwmChannelL2);
  
  // 初始状态下电机停止
  ledcWrite(pwmChannelR1, 0);
  ledcWrite(pwmChannelL1, 0);
  ledcWrite(pwmChannelR2, 0);
  ledcWrite(pwmChannelL2, 0);
  
  Serial.println("System initialized successfully!");
  Serial.println("Use throttle switch to enable motors");
  Serial.println("Potentiometer controls motor speed");
  Serial.println("Joystick X-axis controls steering (15° deadzone)");
}

void loop() {
  // 读取传感器值
  readSensors();
  
  // 处理传感器数据
  processSensorData();
  
  // 控制电机
  controlMotors();
  
  // 在OLED上显示当前页面数据（固定显示页面0）
  display.clearDisplay();
  displayPage0();
  display.display();
  
  delay(50); // 控制刷新频率
}    